//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================
//
// Copyright (c) Microsoft Corporation.  All rights reserved.
//
//
// Use of this source code is subject to the terms of the Microsoft end-user
// license agreement (EULA) under which you licensed this SOFTWARE PRODUCT.
// If you did not accept the terms of the EULA, you are not authorized to use
// this source code. For a copy of the EULA, please see the LICENSE.RTF on your
// install media.
//

// Copyright (c) 2002 BSQUARE Corporation.  All rights reserved.
// DO NOT REMOVE --- BEGIN EXTERNALLY DEVELOPED SOURCE CODE ID 40973--- DO NOT REMOVE

// Miscellaneous Apis

#include <ddk.h>
#include "SDMisc.h"

// Shifts pbInput down by dwBitOffset.
void ShiftBytes(PByte pbInput, UInt32 cbInput, UInt32 dwBitOffset, PByte pbOutput)
{
    PREFAST_DEBUGCHK(pbInput);
    PREFAST_DEBUGCHK(pbOutput);

    UInt32 dwByteIndex = dwBitOffset / 8;
    dwBitOffset %= 8;

    UInt32 dwRemainderShift = 8 - dwBitOffset;

    // Only copy 4 bytes max.
    UInt32 dwEndIndex = min(dwByteIndex + sizeof(UInt32), cbInput);
    UInt32 dwCurrOutputIndex = 0;
    while (dwByteIndex < dwEndIndex) {
        DEBUGCHK(dwCurrOutputIndex < sizeof(UInt32), kprintf("OutputIndex Out Of Range\n"));
        DEBUGCHK(dwByteIndex < cbInput, kprintf("Input Bytes Out Of Range\n"));

        pbOutput[dwCurrOutputIndex] = pbInput[dwByteIndex] >> dwBitOffset;

        ++dwByteIndex;

        if (dwByteIndex != cbInput) {
            Byte bTemp = pbInput[dwByteIndex];
            bTemp <<= dwRemainderShift;
            pbOutput[dwCurrOutputIndex] |= bTemp;
        }

        ++dwCurrOutputIndex;
    }
}


///////////////////////////////////////////////////////////////////////////////
//  GetBitSlice - Get a bit slice from a stream of bytes
//  Input:  pBuffer - buffer containing data stream
//          cbBuffer - size of buffer in bytes
//          dwBitOffset - bit offset from start of buffer
//          ucBitCount - number of bits (less than or equal to 32)
//  Output:
//
//  Return: returns a UInt32 contain the bit slice shifted to fill the least significant bits
//  Notes:
///////////////////////////////////////////////////////////////////////////////
UInt32 GetBitSlice(PUInt8 pBuffer, UInt32 cbBuffer, UInt32 dwBitOffset, UInt8 ucBitCount)
{
    UInt8 rgbShifted[4] = { 0 };

    if (ucBitCount > 32) {
        DEBUG_CHECK(FALSE, (kprintf("GetBitSlice: invalid number of bits \n")));
        return 0;
    }

    if ((dwBitOffset + ucBitCount) > cbBuffer*8) {
        DEBUG_CHECK(FALSE, (kprintf("GetBitSlice: invalid bit offset given the number of bits \n")));
        return 0;
    }

    // Shift the pBuffer down by dwBitOffset bits.
    ShiftBytes(pBuffer, cbBuffer, dwBitOffset, rgbShifted);

    UInt32 dwUsedBytes; // How many bytes have valid data.

    if (ucBitCount % 8 == 0) {
        // Return a byte multiple.
        dwUsedBytes = ucBitCount / 8;
    }
    else {
        // Clear the last used byte of upper bits.
        UInt32 dwLastByteIndex = (ucBitCount - 1) / 8;
        UInt32 dwRemainderShift = 8 - (ucBitCount % 8);
        rgbShifted[dwLastByteIndex] <<= dwRemainderShift;
        rgbShifted[dwLastByteIndex] >>= dwRemainderShift;
        dwUsedBytes = dwLastByteIndex + 1;
    }

    // Clear the unused bytes.
    if (dwUsedBytes != sizeof(rgbShifted)) {
        memset(rgbShifted + dwUsedBytes, 0, sizeof(rgbShifted) - dwUsedBytes);
    }
    UInt32 dwRet;
    memcpy(&dwRet, rgbShifted, sizeof(dwRet));

    return dwRet;
}


UInt32 GetBitSliceEx( PUInt8 pInBuf, UInt32 cbInBuf, PUInt8 pOutBuf,
                   UInt32 dwBitOffset, UInt8 ucBitCount)
{
    UInt32 bInIdx, hInRem, hOutRem, hNeed, bitCnt, bOutIdx, OutIdx, shitf;
    bitCnt  = ucBitCount;
    bOutIdx = 0;
    bInIdx = dwBitOffset;
    UInt8 tmp;

    while (bitCnt) {
        hInRem  =  (8 - bInIdx % 8);
        hOutRem =  (8 - bOutIdx % 8);
        hNeed =  (bitCnt > hOutRem) ? hOutRem : bitCnt;
        shitf =   (hNeed > hInRem) ? (hNeed - hInRem) : (hInRem - hNeed);
        OutIdx = (bOutIdx>>3);
        if ( (bOutIdx % 8) == 0 ) {
            pOutBuf[OutIdx] = 0;
        }
        tmp = (pInBuf[(bInIdx>>3)] & ((1<<hInRem) - 1) );
        if (hNeed > hInRem) {
            tmp <<= shitf;
            shitf = hInRem;
        }
        else {
            tmp >>= shitf;
            shitf = hNeed;
        }
        pOutBuf[OutIdx] |= tmp;
        bitCnt  -= shitf;
        bOutIdx += shitf;
        bInIdx  += shitf;
    }

    return 0;
}


void Big2Little(PUInt8 pData, UInt32 length)
{
    UInt32 hIdx, tIdx;
    UInt8 tmp;
    hIdx = 0, tIdx = length -1 ;

    while (hIdx < tIdx) {
        tmp = pData[hIdx];
        pData[hIdx] = pData[tIdx];
        pData[tIdx] = tmp;
        hIdx++;
        tIdx--;
    }
}

void hexouts(UInt8* buf, UInt32 buflen)
{
    UInt32 linCnt = buflen >> 4;
    UInt32 line, clum = 0;
    if (linCnt == 0) {
        for(clum = 0; clum < buflen; clum++)
            kprintf("%02x ", buf[clum]);
        kprintf("\nbuffer length %d\n", buflen);
        return;
    }
    for (line = 0; line < linCnt; line++) {
        kprintf("%04x\t", line*0x10);
        for(clum = 0; clum < 0x10; clum++)
            kprintf("%02x ", buf[line*0x10+clum]);
        kprintf("\n");
    }
    kprintf("line number : %d, clum : %d, Bytes : %d\n",
        line, clum, line*clum);
}

// DO NOT REMOVE --- END EXTERNALLY DEVELOPED SOURCE CODE ID --- DO NOT REMOVE

